#include "stm32f10x.h"                  // Device header
#include "mpu6050.h"
#include "mpu6050_i2c.h"
#include "main.h"


#define MPU6050_WHO_AM_I_REG        0x75
#define MPU6050_PWR_MGMT_1_REG      0x6B
#define MPU6050_PWR_MGMT_2_REG      0x6C
#define MPU6050_CONFIG_REG          0x1A
#define MPU6050_SMPLRT_DIV_REG      0x19
#define MPU6050_ACCEL_CONFIG_REG    0x1C
#define MPU6050_GYRO_CONFIG_REG     0x1B

#define MPU6050_ACCEL_XOUT_H_REG    0x3B
#define MPU6050_TEMP_OUT_H_REG      0x41
#define MPU6050_GYRO_XOUT_H_REG     0x43

#define MPU6050_ADDR    0x68

bool mpu6050_init(void)
{
    mpu6050_i2c_init();

    uint8_t whoami;
    mpu6050_i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I_REG, &whoami, 1);
    if(whoami != MPU6050_ADDR)
    {
        return false;
    }

    uint8_t data;

    data = 0;
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_PWR_MGMT_1_REG, &data, 1);
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_PWR_MGMT_2_REG, &data, 1);

    data = 0x06;
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_CONFIG_REG, &data, 1);

    data = 0x00;
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_SMPLRT_DIV_REG, &data, 1);

    data = 0x00;
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_ACCEL_CONFIG_REG, &data, 1);

    data = 0x00;
    mpu6050_i2c_write(MPU6050_ADDR, MPU6050_GYRO_CONFIG_REG, &data, 1);

    return true;
}
void mpu6050_read_accel(mpu6050_accel_t *accel)
{
    uint8_t raw_data[6];

    mpu6050_i2c_read(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H_REG, raw_data, 6);

    accel->x_raw = (int16_t)((raw_data[0] << 8) | raw_data[1]);
    accel->y_raw = (int16_t)((raw_data[2] << 8) | raw_data[3]);
    accel->z_raw = (int16_t)((raw_data[4] << 8) | raw_data[5]);

    accel->x = accel->x_raw / 16384.0f;
    accel->y = accel->y_raw / 16384.0f;
    accel->z = accel->z_raw / 16384.0f;
}

void mpu6050_read_gyro(mpu6050_gyro_t *gyro)
{
    uint8_t raw_data[6];

    mpu6050_i2c_read(MPU6050_ADDR, MPU6050_ACCEL_XOUT_H_REG, raw_data, 6);

    gyro->x_raw = (int16_t)((raw_data[0] << 8) | raw_data[1]);
    gyro->y_raw = (int16_t)((raw_data[2] << 8) | raw_data[3]);
    gyro->z_raw = (int16_t)((raw_data[4] << 8) | raw_data[5]);

    gyro->x = gyro->x_raw / 131.0f;
    gyro->y = gyro->y_raw / 131.0f;
    gyro->z = gyro->z_raw / 131.0f;
}
float mpu6050_read_temper(void)
{
    uint8_t raw_data[2];

    mpu6050_i2c_read(MPU6050_ADDR, MPU6050_TEMP_OUT_H_REG, raw_data, 2);

    int16_t temp_raw = (int16_t)((raw_data[0] << 8) | raw_data[1]);

    return (float)temp_raw / 340.0f + 36.53f;
}
